use r2r::unitree_go::msg::LowCmd;

// Error types for CRC operations
#[derive(Debug, Clone, PartialEq)]
pub enum CrcError {
    InvalidInput(String),
    BufferTooSmall,
    AlignmentError,
}

impl std::fmt::Display for CrcError {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        match self {
            CrcError::InvalidInput(msg) => write!(f, "Invalid input: {}", msg),
            CrcError::BufferTooSmall => write!(f, "Buffer too small for CRC calculation"),
            CrcError::AlignmentError => write!(f, "Buffer alignment error"),
        }
    }
}

impl std::error::Error for CrcError {}

pub type CrcResult<T> = Result<T, CrcError>;

// Constants from motor_crc.h
pub const HIGHLEVEL: u8 = 0xee;
pub const LOWLEVEL: u8 = 0xff;
pub const TRIGERLEVEL: u8 = 0xf0;
pub const POS_STOP_F: f64 = 2.146E+9;
pub const VEL_STOP_F: f64 = 16000.0;

// Joint index constants
pub const FR_0: usize = 0;
pub const FR_1: usize = 1;
pub const FR_2: usize = 2;
pub const FL_0: usize = 3;
pub const FL_1: usize = 4;
pub const FL_2: usize = 5;
pub const RR_0: usize = 6;
pub const RR_1: usize = 7;
pub const RR_2: usize = 8;
pub const RL_0: usize = 9;
pub const RL_1: usize = 10;
pub const RL_2: usize = 11;

// CRC32 implementation matching the C++ version
pub fn crc32_core(ptr: &[u32]) -> CrcResult<u32> {
    if ptr.is_empty() {
        return Err(CrcError::InvalidInput("Empty input slice".to_string()));
    }
    let mut crc: u32 = 0xFFFF_FFFF;
    let poly: u32 = 0x04c11db7;
    for &word in ptr {
        let mut xbit: u32 = 1 << 31;
        let data = word;
        for _ in 0..32 {
            if (crc & 0x8000_0000) != 0 {
                crc = (crc << 1) ^ poly;
            } else {
                crc <<= 1;
            }
            if (data & xbit) != 0 {
                crc ^= poly;
            }
            xbit >>= 1;
        }
    }
    Ok(crc)
}

// Convert LowCmd into raw 32-bit word slice and compute crc like C++ version
pub fn get_crc(msg: &mut LowCmd) -> CrcResult<()> {
    // Validate motor_cmd array length
    if msg.motor_cmd.len() != 20 {
        return Err(CrcError::InvalidInput(
            format!("Expected 20 motor commands, got {}", msg.motor_cmd.len())
        ));
    }
    
    // Validate array sizes
    if msg.head.len() != 2 {
        return Err(CrcError::InvalidInput("Invalid head array size".to_string()));
    }
    if msg.sn.len() != 2 {
        return Err(CrcError::InvalidInput("Invalid SN array size".to_string()));
    }
    if msg.version.len() != 2 {
        return Err(CrcError::InvalidInput("Invalid version array size".to_string()));
    }
    if msg.wireless_remote.len() != 40 {
        return Err(CrcError::InvalidInput("Invalid wireless_remote array size".to_string()));
    }
    if msg.led.len() != 12 {
        return Err(CrcError::InvalidInput("Invalid led array size".to_string()));
    }
    if msg.fan.len() != 2 {
        return Err(CrcError::InvalidInput("Invalid fan array size".to_string()));
    }
    if msg.bms_cmd.reserve.len() != 3 {
        return Err(CrcError::InvalidInput("Invalid bms_cmd.reserve array size".to_string()));
    }
    
    // Validate motor_cmd reserve arrays
    for (i, m) in msg.motor_cmd.iter().enumerate() {
        if m.reserve.len() != 3 {
            return Err(CrcError::InvalidInput(
                format!("Invalid motor_cmd[{}].reserve array size", i)
            ));
        }
    }
    // Build a raw layout identical to the C++ LowCmd struct
    // We cannot rely on repr(C) here, so we assemble a byte buffer
    // in the exact field order and sizes, then compute crc over u32 words
    
    // Use stack-allocated buffer for better performance
    // Estimated size: 2 + 1 + 1 + 8 + 8 + 2 + 20*(1+4*5+12) + 1+3 + 40 + 12 + 2 + 1 + 4 = ~600 bytes
    const MAX_BUFFER_SIZE: usize = 1024; // Conservative estimate with padding
    let mut buf: [u8; MAX_BUFFER_SIZE] = [0; MAX_BUFFER_SIZE];
    let mut pos: usize = 0;
    
    // Helper macro for safe buffer writing
    macro_rules! write_to_buf {
        ($data:expr) => {
            let data = $data;
            if pos + data.len() > MAX_BUFFER_SIZE {
                return Err(CrcError::BufferTooSmall);
            }
            buf[pos..pos + data.len()].copy_from_slice(data);
            pos += data.len();
        };
    }

    // head[2]
    write_to_buf!(&msg.head);
    // level_flag, frame_reserve
    write_to_buf!(&[msg.level_flag]);
    write_to_buf!(&[msg.frame_reserve]);

    // sn[2], version[2] as u32 native endian (matching C++ memcpy behavior)
    for v in &msg.sn { write_to_buf!(&v.to_ne_bytes()); }
    for v in &msg.version { write_to_buf!(&v.to_ne_bytes()); }

    // bandwidth u16
    write_to_buf!(&msg.bandwidth.to_ne_bytes());

    // motor_cmd[20]
    for m in &msg.motor_cmd {
        write_to_buf!(&[m.mode]);
        write_to_buf!(&m.q.to_ne_bytes());
        write_to_buf!(&m.dq.to_ne_bytes());
        write_to_buf!(&m.tau.to_ne_bytes());
        write_to_buf!(&m.kp.to_ne_bytes());
        write_to_buf!(&m.kd.to_ne_bytes());
        for r in &m.reserve { write_to_buf!(&r.to_ne_bytes()); }
    }

    // bms_cmd
    write_to_buf!(&[msg.bms_cmd.off]);
    write_to_buf!(&msg.bms_cmd.reserve);

    // wireless_remote[40]
    write_to_buf!(&msg.wireless_remote);

    // led[12]
    write_to_buf!(&msg.led);

    // fan[2]
    write_to_buf!(&msg.fan);

    // gpio u8
    write_to_buf!(&[msg.gpio]);

    // reserve u32
    write_to_buf!(&msg.reserve.to_ne_bytes());

    // Pad to 4-byte alignment before computing crc over u32 words
    while pos % 4 != 0 { 
        if pos >= MAX_BUFFER_SIZE {
            return Err(CrcError::BufferTooSmall);
        }
        buf[pos] = 0;
        pos += 1;
    }

    // Compute len in u32 words minus last word for crc itself, like C++ (sizeof(LowCmd)>>2)-1
    // Our buffer currently excludes msg.crc; that's correct.
    if pos % 4 != 0 {
        return Err(CrcError::AlignmentError);
    }
    
    let words: &[u32] = unsafe {
        std::slice::from_raw_parts(buf.as_ptr() as *const u32, pos / 4)
    };

    let crc = crc32_core(words)?;
    msg.crc = crc;
    Ok(())
}